#include "task/task.h"

using namespace task;

TaskProcess::TaskProcess(ros::NodeHandle &nh, ros::NodeHandle &private_nh)
    : nh_(nh),
      private_nh_(private_nh)
{
    Init();
}

TaskProcess::~TaskProcess()
{
    common_lib::SpdLog::Instance()->EndLog();
}

void TaskProcess::Init()
{
    private_nh_.getParam("map_file", map_file_);
    private_nh_.getParam("param_file", vehicle_param_file_);
    private_nh_.getParam("path", file_path_);
    common_lib::SpdLog::Instance()->InitSpdLog(file_path_ + "/log", "task");
    LINFO("================================== Task Start ==================================");
    local_host_task_times_ = 10;
    // YAML::Node vehicle_params = YAML::LoadFile(vehicle_param_file_);
    // communication_type_       = vehicle_params["communication_type"].as<std::string>();

    // if (communication_type_ == "localhost")
    // {
    // }

    sub_actuator_ = nh_.subscribe("/actuator", 10, &TaskProcess::ActuatorCallback, this);
    sub_control_ = nh_.subscribe("/control", 10, &TaskProcess::ControlCallback, this);
    sub_control_state_ = nh_.subscribe("/control_state", 10, &TaskProcess::ControlStateCallback, this);

    pub_task_ = nh_.advertise<task_msgs::task_msg>("/task", 10);

    srv_client_path_info_ = nh_.serviceClient<map_operation_msgs::GetPathInfo>("/GetPathInfoSrv");
    srv_client_point_info_ = nh_.serviceClient<map_operation_msgs::GetPointInfo>("/GetPointInfoSrv");

    ptr_cmd_process_ = std::make_shared<task::CommandProcess>(this);
    ptr_cmd_process_->Init(vehicle_param_file_);
}

void TaskProcess::Start()
{
    ros::Rate loop(1);
    while (ros::ok())
    {
        LocalHostProcess();
        ros::spinOnce();
        loop.sleep();
    }
}

void TaskProcess::LocalHostProcess()
{
    double now_time = ros::Time::now().toSec();
    static double last_time = now_time;
    if (now_time - last_time < 5.0)
    {
        return;
    }

    last_time = now_time;

    if (amr_state_.control_state.amr_fsm_state == "idle")
    {
        ROS_WARN("LocalHostProcess() --> task_id: %d %d", amr_state_.control_state.task_id, task_receive_.task_id);
    }

    if (local_host_task_cnt_ < local_host_task_times_)
    {
        // 满足如下情况才下发任务:
        //  1. AMR车端状态为空闲
        //  2. 自动模式
        //  3. 当前的任务id为上一次任务的id, 或者为0(表示任务失败)
        if (amr_state_.control_state.amr_fsm_state == "idle" &&
            amr_state_.actuator.is_auto_mode &&
            (amr_state_.control_state.task_id == task_receive_.task_id || amr_state_.control_state.task_id == 0))
        {
            local_host_task_cnt_++;
            TaskInteract();
        }
    }
}

void TaskProcess::ControlCallback(const control_msgs::control_msgConstPtr &msg)
{
    amr_state_.control = *msg;
}

void TaskProcess::ControlStateCallback(const control_msgs::control_state_msgConstPtr &msg)
{
    amr_state_.control_state = *msg;
}

void TaskProcess::ActuatorCallback(const actuator_msgs::actuator_msgConstPtr &msg)
{
    amr_state_.actuator = *msg;
}

bool TaskProcess::GetPathInofSrv(map_operation_msgs::GetPathInfoRequest &req, map_operation_msgs::GetPathInfoResponse &resp)
{
    if (!srv_client_path_info_.exists())
        return false;

    if (srv_client_path_info_.waitForExistence(ros::Duration(0.5f)))
    {
        int cnt = 0;
        while (!srv_client_path_info_.call(req, resp) && (cnt < 5))
        {
            cnt++;
            if (cnt == 5)
            {
                throw("error");
            }
            sleep(1);
        }
    }
    return resp.is_valid;
}

bool TaskProcess::GetPathInofSrv(map_operation_msgs::GetPathInfo &srv)
{
    if (!srv_client_path_info_.exists())
        return false;
    if (srv_client_path_info_.waitForExistence(ros::Duration(0.5f)))
    {
        int cnt = 0;
        // printf("TaskProcess::GetPathInofSrv() cnt: %d\n", cnt);
        while (!srv_client_path_info_.call(srv) && (cnt < 5))
        {
            cnt++;
            if (cnt == 5)
            {
                throw("error");
                return false;
            }
            sleep(1);
        }
    }
    return srv.response.is_valid;
}

bool TaskProcess::GetPointInfoSrv(map_operation_msgs::GetPointInfoRequest &req, map_operation_msgs::GetPointInfoResponse &resp)
{
    if (!srv_client_point_info_.exists())
        return false;
    if (srv_client_point_info_.waitForExistence(ros::Duration(0.5f)))
    {
        int cnt = 0;
        while (!srv_client_point_info_.call(req, resp) && (cnt < 5))
        {
            cnt++;
            if (cnt == 5)
            {
                throw("error");
            }
            sleep(1);
        }
    }
    return resp.is_valid;
}

bool TaskProcess::GetPointInfoSrv(map_operation_msgs::GetPointInfo &srv)
{
    if (!srv_client_point_info_.exists())
        return false;
    if (srv_client_point_info_.waitForExistence(ros::Duration(0.5f)))
    {
        int cnt = 0;
        while (!srv_client_point_info_.call(srv) && (cnt < 5))
        {
            cnt++;
            if (cnt == 5)
            {
                printf("TaskProcess::GetPointInfoSrv() error cnt %s\n", srv.request.pointid.c_str());
            }
            sleep(1);
        }
    }
    return srv.response.is_valid;
}

bool TaskProcess::GetPointInfoSrv(std::string &pointid, sTaskPoint &point)
{
    map_operation_msgs::GetPointInfoResponse resp;
    resp.is_valid = false;
    if (!srv_client_point_info_.exists())
        return resp.is_valid;

    map_operation_msgs::GetPointInfoRequest req;
    req.pointid = pointid;
    if (!GetPointInfoSrv(req, resp))
    {
        resp.is_valid = false;
    }
    else
    {
        point.x = resp.pose.position.x;
        point.y = resp.pose.position.y;
        point.z = resp.pose.position.z;
        point.theta = tf::getYaw(resp.pose.orientation);
        point.point_id = pointid;
    }
    return resp.is_valid;
}

void TaskProcess::TaskInteract()
{
    task::sTaskMsg task_msg;
    ptr_cmd_process_->TaskCmdProcess(amr_state_, task_msg);

    // TODO
    // bool is_recevied_same_task_ = false;
    // if (task_receive_.command == "working" &&
    //     task_receive_.task_id == amr_state_.control_state.task_id &&
    //     ((amr_state_.control_state.amr_fsm_state == "working") || (amr_state_.control_state.amr_fsm_state == "charging")))
    // {
    //     is_recevied_same_task_ = true;
    // }

    if (task_msg.v_task_path.size() > 0)
    {
        task_receive_.task_id = task_msg.task_id;
        task_receive_.target_id = task_msg.target_point_id;

        // Change Struct To Msg
        task_msgs::task_msg msg_task;
        StructMsgToRosMsg(msg_task, task_msg);
        pub_task_.publish(msg_task);
    }
}

void TaskProcess::StructMsgToRosMsg(task_msgs::task_msg &msg_ros, task::sTaskMsg &msg_struct)
{
    msg_ros.header.stamp = ros::Time::now();

    msg_ros.target_pointid = msg_struct.target_point_id;
    msg_ros.task_id = msg_struct.task_id;
    for (auto task : msg_struct.v_task_path)
    {
        task_msgs::task_path path;
        path.max_speed = task.max_speed;
        path.pathid = task.path_id;
        path.task_id = task.task_id;
        path.pathtype = task.path_type;
        path.move_direction = task.move_direction;
        path.amr_angle = task.amr_angle;
        path.angle_type = task.angle_type;

        for (auto p : task.v_control_points)
        {
            task_msgs::task_point point;
            point.pointid = p.point_id;
            point.pose.position.x = p.x;
            point.pose.position.y = p.y;
            point.pose.position.z = p.z;
            point.pose.orientation = tf::createQuaternionMsgFromYaw(p.theta);

            path.control_points.push_back(point);
        }

        for (auto p : task.v_key_points)
        {
            task_msgs::task_point point;
            point.pointid = p.point_id;
            point.pose.position.x = p.x;
            point.pose.position.y = p.y;
            point.pose.position.z = p.z;
            point.pose.orientation = tf::createQuaternionMsgFromYaw(p.theta);

            for (auto w : p.v_work_task)
            {
                task_msgs::work_task work;
                work.control_mode = w.control_mode;
                work.enable = w.enable;
                work.id = w.work_id;
                work.is_receive_cmd = w.is_receive_cmd;
                work.pose = w.pose;
                work.speed = w.speed;
                work.thresh = w.thresh;
                work.type = w.type;
                work.value = w.value;
                point.work_task.push_back(work);
            }

            path.key_points.push_back(point);
        }
        msg_ros.task.push_back(path);
    }
}